// src/sensor_data_subscriber.cpp
#include "sensor_data_subscriber.h"
#include <QDebug>
#include <chrono>
#include <thread>
#include <unistd.h>
#include "../global_logger.h"  // 添加全局日志头文件
using namespace std::literals::chrono_literals;

/**
 * @brief SensorDataSubscriber类的构造函数
 * @param parent 父对象指针，用于Qt对象树管理
 * 
 * 初始化SensorDataSubscriber对象，创建工作者对象并设置线程间通信
 */
SensorDataSubscriber::SensorDataSubscriber(QObject *parent)
    : QObject(parent)
    , worker_(new SensorDataWorker)
{
    // 将工作者移动到独立线程
    worker_->moveToThread(&workerThread_);
    
    // 连接信号和槽
    connect(&workerThread_, &QThread::started, worker_, &SensorDataWorker::start);
    // 使用Qt::QueuedConnection确保线程安全
    connect(worker_, &SensorDataWorker::sensorDataUpdated, 
        this, &SensorDataSubscriber::sensorDataUpdated,
        Qt::QueuedConnection);
    connect(worker_, &SensorDataWorker::finished, &workerThread_, &QThread::quit);
    connect(&workerThread_, &QThread::finished, worker_, &QObject::deleteLater);
    
    LOG_INFO("SensorDataSubscriber 初始化完成");
}

/**
 * @brief SensorDataSubscriber类的析构函数
 * 
 * 负责安全地停止工作线程并清理资源
 */
SensorDataSubscriber::~SensorDataSubscriber() {
    LOG_DEBUG("销毁 SensorDataSubscriber");
    worker_->stop();    // 先停止工作逻辑
    workerThread_.quit(); // 再通知线程退出
    workerThread_.wait(1000); // 等待线程安全结束
    LOG_DEBUG("SensorDataSubscriber 销毁完成");
}

/**
 * @brief 启动传感器数据订阅
 * 
 * 启动工作线程，开始处理传感器数据
 */
void SensorDataSubscriber::start()
{
    LOG_INFO("SensorDataWorker 启动传感器数据订阅");
    workerThread_.start();
}

/**
 * @brief 停止传感器数据订阅
 * 
 * 停止工作者的数据处理逻辑
 */
void SensorDataSubscriber::stop()
{
    LOG_INFO("SensorDataWorker 停止传感器数据订阅");
    worker_->stop();
}

/**
 * @brief 设置机器人命名空间（根据机器人ID）
 * 
 * 设置机器人ID并重新订阅话题
 */
void SensorDataSubscriber::setNamespace(const QString &robotId)
{
    LOG_INFO("SensorDataWorker 设置机器人ID: {}", robotId.toStdString());
    worker_->setNamespace(robotId);
}

/**
* SensorDataWorker 实现
*/
/**
 * @brief SensorDataWorker类的构造函数
 * @param parent 父对象指针，用于Qt对象树管理
 */
SensorDataWorker::SensorDataWorker(QObject *parent)
    : QObject(parent)
    , running_(false)
{
    LOG_DEBUG("SensorDataWorker 构造完成");
}

/**
 * @brief SensorDataWorker类的析构函数
 * 在销毁前停止线程运行
 */
SensorDataWorker::~SensorDataWorker()
{
    LOG_DEBUG("销毁 SensorDataWorker");
    stop();
}

/**
 * @brief 设置机器人ID（作为命名空间）并根据需要重新订阅传感器数据话题
 * 
 * 当机器人ID发生变化时，该函数会更新内部存储的机器人ID，
 * 并在节点正在运行的情况下重新订阅相关的传感器话题。
 * 如果ROS节点尚未创建但已有有效的机器人ID，则会创建新节点和执行器。
 * 
 * @param robotId 新的机器人ID字符串
 */
void SensorDataWorker::setNamespace(const QString &robotId)
{
    std::string oldRobotId = robotId_;
    if (robotId_.compare(robotId.toStdString()) != 0) {
        robotId_ = robotId.toStdString();
        LOG_INFO("更新机器人ID为: {}", robotId_);
        
        // 如果已经在运行，则重新订阅话题
        if (running_.load()) {
            // 如果节点已经创建，重新订阅话题
            if (node_) {
                subscribeTopic();
            } else {
                // 如果还没有创建节点，但现在有了robotId，则创建节点
                if (!robotId_.empty()) {
                    node_ = std::make_shared<rclcpp::Node>(NODE_NAME);
                    LOG_DEBUG("创建ROS节点: {}", NODE_NAME);
                    
                    // 创建执行器并添加节点
                    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
                    executor_->add_node(node_);
                    LOG_DEBUG("创建执行器并添加节点");
                    
                    // 订阅话题
                    subscribeTopic();
                }
            }
        }
    }
}

/**
 * @brief 订阅话题
 * 
 * 根据机器人ID创建或重新创建订阅者
 */
void SensorDataWorker::subscribeTopic()
{
    if (!node_) {
        LOG_WARN("节点未创建，无法订阅话题");
        return;
    }
    
    // 如果已有订阅者，先重置它
    if (subscription_) {
        subscription_.reset();
    }
    
    // 构建话题名称
    std::string topic_name = "/" + robotId_ + "/env_sensor_data";
    LOG_INFO("订阅话题: {}", topic_name);
    
    // 创建订阅者
    subscription_ = node_->create_subscription<rms_pkg::msg::SensorData>(
        topic_name, 10,
        [this](const rms_pkg::msg::SensorData::SharedPtr msg) {
            try{
                // 将数据转换为QString并发出信号，包括robot_id
                QString robotId = QString::fromStdString(msg->robot_id);
                QString airTemperature = QString::number(msg->air_temperature, 'f', 2) + "℃";
                QString airHumidity = QString::number(msg->air_humidity, 'f', 2) + "%";
                QString lightIntensity = QString::number(msg->light_intensity, 'f', 0) + " Lux";
                QString co2Concentration = QString::number(msg->co2_concentration) + " ppm";
                QString soilTemperature = QString::number(msg->soil_temperature, 'f', 2) + "℃";
                QString soilMoisture = QString::number(msg->soil_moisture, 'f', 1) + "%";
                
                LOG_DEBUG("接收到传感器数据 - Robot ID: {}", msg->robot_id);

                emit sensorDataUpdated(robotId, airTemperature, airHumidity, lightIntensity,
                                    co2Concentration, soilTemperature, soilMoisture);
            } catch (const std::exception& e) {
                LOG_ERROR("回调函数异常: {}", e.what());
            }
        });
    LOG_DEBUG("创建传感器数据订阅者");
}

/**
 * @brief 启动传感器数据处理工作线程
 * 设置运行标志为true并开始执行run方法
 */
void SensorDataWorker::start()
{
    LOG_INFO("启动 SensorDataWorker");
    running_ = true;
    run();
}

/**
 * @brief 停止传感器数据处理工作线程
 * 设置运行标志为false，并取消执行器等待状态以强制退出
 */
void SensorDataWorker::stop()
{
    LOG_INFO("停止 SensorDataWorker");
    running_.store(false);
    if (executor_ && node_) executor_->cancel(); // 强制退出等待
}

/**
 * @brief 执行传感器数据订阅和处理的主要逻辑
 * 创建ROS节点、订阅者和执行器，在循环中处理消息直到被停止
 */
void SensorDataWorker::run()
{
    try {
        LOG_INFO("SensorDataWorker 开始运行 robotId: {}", robotId_);
        
        // 只有当robotId不为空时才创建节点
        if (!robotId_.empty()) {
            // 在工作线程中创建ROS节点
            node_ = std::make_shared<rclcpp::Node>(NODE_NAME);  // 不指定命名空间
            LOG_DEBUG("创建ROS节点: {}", NODE_NAME);
            
            // 订阅话题
            subscribeTopic();
            
            // 创建执行器
            executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
            executor_->add_node(node_);
            LOG_DEBUG("创建执行器并添加节点");
        } else {
            LOG_WARN("robotId为空，暂不创建节点和订阅话题");
        }
        
        // 在工作线程中运行执行器 - 使用正确的spin方法
        while (running_.load() && rclcpp::ok()) {
            // 使用spin_some处理可用的消息
            if (executor_) {
                executor_->spin_some();  // 处理ROS消息
            }
            std::this_thread::sleep_for(10ms); // 显式休眠降低CPU占用
        }
        LOG_DEBUG("SensorDataWorker 执行循环结束");
    } catch (const std::exception& e) {
        LOG_ERROR("SensorDataWorker 运行异常: {}", e.what());
    }
    
    LOG_INFO("SensorDataWorker 运行完成");
    emit finished();
}